/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>
#include "voslam_calc.h"
#include "voslam_blas.h"
#include "voslam_util.h"

/* gcc -O2 -Wall -g -c -o voslam_calc.o voslam_calc.c */

void slam_vx_icb(double *A,double *B,double *C,int m,int k,int n)
{
      cblas_dgemm( CblasColMajor, CblasTrans, CblasNoTrans, m, n, k, -1.0, A, k, B, k, 1.0, C, m );
}

void proj3D_fun_er(
        double *e,
        double *depth,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind
        )
{
    double P[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;

    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*X[3*ind]+P[3]*X[1+3*ind]+P[6]*X[2+3*ind]+P[9];
    y=P[1]*X[3*ind]+P[4]*X[1+3*ind]+P[7]*X[2+3*ind]+P[10];
    z=P[2]*X[3*ind]+P[5]*X[1+3*ind]+P[8]*X[2+3*ind]+P[11];
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    depth[0]=z;

}

void proj3D_fun_er1(
        double *e,
        double *depth,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind
        )
{
    double P[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;

    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*(X[3*ind]+P[9])+P[3]*(X[1+3*ind]+P[10])+P[6]*(X[2+3*ind]+P[11]);
    y=P[1]*(X[3*ind]+P[9])+P[4]*(X[1+3*ind]+P[10])+P[7]*(X[2+3*ind]+P[11]);
    z=P[2]*(X[3*ind]+P[9])+P[5]*(X[1+3*ind]+P[10])+P[8]*(X[2+3*ind]+P[11]);
    
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    depth[0]=z;

}

void proj3D_fun(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int D=10;


    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
	/* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];

    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
	dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
	dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
	dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
	dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
	dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
	dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
	dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
	dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
	dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
	dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
	dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
	dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
	dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
	dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
	dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
	dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
	dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
	dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
	dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
	dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
	dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
	dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
	dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
	dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
	dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
	dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];
	
	P[0]*=norm;
	P[1]*=norm;
	P[2]*=norm;
	P[3]*=norm;
	P[4]*=norm;
	P[5]*=norm;
	P[6]*=norm;
	P[7]*=norm;
	P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
	x=P[0]*X[3*ind]+P[3]*X[1+3*ind]+P[6]*X[2+3*ind]+P[9];
	y=P[1]*X[3*ind]+P[4]*X[1+3*ind]+P[7]*X[2+3*ind]+P[10];
	z=P[2]*X[3*ind]+P[5]*X[1+3*ind]+P[8]*X[2+3*ind]+P[11];
    if(z==0)
	    z=-0.000000001; /* fixme */
    /*residuals */
	e[0]=pts[0]-p[1]-p[0]*x/z;
	e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
		e[0]=th;
    if(e[0]<-th)
		e[0]=-th;
	if(e[1]>th)
		e[1]=th;
    if(e[1]<-th)
		e[1]=-th;	
    if(z<0)
    {
      /* need to handle this properly 
      i.e e=0-z ? and initialising*/
      e[0]=th;
      e[1]=th;
    }
    /* help variables ? necessary */
	z2=z*z;
	f1=-p[0]*x/z2;
	f2=-p[0]*y/z2;
	dudP0=p[0]*X[3*ind]/z;
	dudP3=p[0]*X[1+3*ind]/z;
	dudP6=p[0]*X[2+3*ind]/z;
	dudP2=f1*X[3*ind];
	dudP5=f1*X[1+3*ind];
	dudP8=f1*X[2+3*ind];
        
	dvdP1=p[0]*X[3*ind]/z;
	dvdP4=p[0]*X[1+3*ind]/z;
	dvdP7=p[0]*X[2+3*ind]/z;
	dvdP2=f2*X[3*ind];
	dvdP5=f2*X[1+3*ind];
	dvdP8=f2*X[2+3*ind];
        
    /* Jacobian */
        /*
        J[0]=x/z;
        J[1]=1.;
        J[2]=0.;
         */
        
    /* motion parameters for u */
	J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
	J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
	J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
	J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
	J[4]=p[0]/z;       //t1
	J[5]=0.;           //t2
	J[6]=f1;           //t3
    /* structure parameters for u */
	J[7]=p[0]*P[0]/z+f1*P[2];
	J[8]=p[0]*P[3]/z+f1*P[5];
	J[9]=p[0]*P[6]/z+f1*P[8];
    /*
        J[D]=y/z;
        J[D+1]=0.;
        J[D+2]=1.;
          
        */
    /* motion parameters for v */
	J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
	J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
	J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
	J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
	J[D+4]=0.;           /* t1 */
	J[D+5]=p[0]/z;       /* t2 */
	J[D+6]=f2;           /* t3 */
    /* structure parameters for v */
	J[D+7]=p[0]*P[1]/z+f2*P[2];
	J[D+8]=p[0]*P[4]/z+f2*P[5];
	J[D+9]=p[0]*P[7]/z+f2*P[8];
        
 
 
}
void proj3D_fun1(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int D=10;
    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];
    
    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*(X[3*ind]+P[9])+P[3]*(X[1+3*ind]+P[10])+P[6]*(X[2+3*ind]+P[11]);
    y=P[1]*(X[3*ind]+P[9])+P[4]*(X[1+3*ind]+P[10])+P[7]*(X[2+3*ind]+P[11]);
    z=P[2]*(X[3*ind]+P[9])+P[5]*(X[1+3*ind]+P[10])+P[8]*(X[2+3*ind]+P[11]);
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;   
    /* help variables ? necessary */
    z2=z*z;
    f1=-p[0]*x/z2;
    f2=-p[0]*y/z2;
    dudP0=p[0]*(X[3*ind]+P[9])/z;
    dudP3=p[0]*(X[1+3*ind]+P[10])/z;
    dudP6=p[0]*(X[2+3*ind]+P[11])/z;
    dudP2=f1*(X[3*ind]+P[9]);
    dudP5=f1*(X[1+3*ind]+P[10]);
    dudP8=f1*(X[2+3*ind]+P[11]);
        
    dvdP1=p[0]*(X[3*ind]+P[9])/z;
    dvdP4=p[0]*(X[1+3*ind]+P[10])/z;
    dvdP7=p[0]*(X[2+3*ind]+P[11])/z;
    dvdP2=f2*(X[3*ind]+P[9]);
    dvdP5=f2*(X[1+3*ind]+P[10]);
    dvdP8=f2*(X[2+3*ind]+P[11]);
        
    /* Jacobian */
        
    /* motion parameters for u */
    J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
    J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
    J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
    J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
    J[4]=p[0]*P[0]/z+f1*P[2];       //t1
    J[5]=p[0]*P[3]/z+f1*P[5];           //t2
    J[6]=p[0]*P[6]/z+f1*P[8];           //t3
    /* structure parameters for u */
    J[7]=p[0]*P[0]/z+f1*P[2];
    J[8]=p[0]*P[3]/z+f1*P[5];
    J[9]=p[0]*P[6]/z+f1*P[8];
    
    /* motion parameters for v */
    J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
    J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
    J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
    J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
    J[D+4]=p[0]*P[1]/z+f2*P[2];           /* t1 */
    J[D+5]=p[0]*P[4]/z+f2*P[5];       /* t2 */
    J[D+6]=p[0]*P[7]/z+f2*P[8];           /* t3 */
    /* structure parameters for v */
    J[D+7]=p[0]*P[1]/z+f2*P[2];
    J[D+8]=p[0]*P[4]/z+f2*P[5];
    J[D+9]=p[0]*P[7]/z+f2*P[8];
}

void stereo_proj3D_fun_er(
        double *e,
        double *depth,
        double *q,
        double *t,
        double *p,
        double *R,
        double *t0,               
        double *pts,
        double *X,
        int ind
        )
{
    double P[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,Xr[3];

    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    Xr[0]=R[0]*X[3*ind]+R[3]*X[1+3*ind]+R[6]*X[2+3*ind]+t0[0];
    Xr[1]=R[1]*X[3*ind]+R[4]*X[1+3*ind]+R[7]*X[2+3*ind]+t0[1];
    Xr[2]=R[2]*X[3*ind]+R[5]*X[1+3*ind]+R[8]*X[2+3*ind]+t0[2];
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*Xr[0]+P[3]*Xr[1]+P[6]*Xr[2]+P[9];
    y=P[1]*Xr[0]+P[4]*Xr[1]+P[7]*Xr[2]+P[10];
    z=P[2]*Xr[0]+P[5]*Xr[1]+P[8]*Xr[2]+P[11];
    if(z==0)
        z=0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    depth[0]=z;
}

void stereo_proj3D_fun(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *R,
        double *t0,               
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,Xr[3];
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    double dvdz,dvdy,dudz,dudx,dudxr0,dudxr1,dudxr2,dvdxr0,dvdxr1,dvdxr2,dxr0dx0,dxr0dx1,dxr0dx2,dxr1dx0,dxr1dx1,dxr1dx2,dxr2dx0,dxr2dx1,dxr2dx2;
    int D=10;
    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];
    
    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    Xr[0]=R[0]*X[3*ind]+R[3]*X[1+3*ind]+R[6]*X[2+3*ind]+t0[0];
    Xr[1]=R[1]*X[3*ind]+R[4]*X[1+3*ind]+R[7]*X[2+3*ind]+t0[1];
    Xr[2]=R[2]*X[3*ind]+R[5]*X[1+3*ind]+R[8]*X[2+3*ind]+t0[2];
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*Xr[0]+P[3]*Xr[1]+P[6]*Xr[2]+P[9];
    y=P[1]*Xr[0]+P[4]*Xr[1]+P[7]*Xr[2]+P[10];
    z=P[2]*Xr[0]+P[5]*Xr[1]+P[8]*Xr[2]+P[11];
    if(z==0)
        z=0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;   
    /* help variables ? necessary */

    z2=z*z;
    dudxr0=p[0]*(P[0]/z-x*P[2]/z2);
    dudxr1=p[0]*(P[3]/z-x*P[5]/z2);
    dudxr2=p[0]*(P[6]/z-x*P[8]/z2);
    dvdxr0=p[0]*(P[1]/z-y*P[2]/z2);
    dvdxr1=p[0]*(P[4]/z-y*P[5]/z2);
    dvdxr2=p[0]*(P[7]/z-y*P[8]/z2);
    dxr0dx0=R[0];
    dxr0dx1=R[3];
    dxr0dx2=R[6];
    dxr1dx0=R[1];
    dxr1dx1=R[4];
    dxr1dx2=R[7];
    dxr2dx0=R[2];
    dxr2dx1=R[5];
    dxr2dx2=R[8];

    dudz=-p[0]*x/z2;
    dudx=p[0]/z;
    dvdz=-p[0]*y/z2;
    dvdy=p[0]/z;
    
    dudP0=p[0]*Xr[0]/z;
    dudP3=p[0]*Xr[1]/z;
    dudP6=p[0]*Xr[2]/z;
    dudP2=dudz*Xr[0];
    dudP5=dudz*Xr[1];
    dudP8=dudz*Xr[2];
        
    dvdP1=p[0]*Xr[0]/z;
    dvdP4=p[0]*Xr[1]/z;
    dvdP7=p[0]*Xr[2]/z;
    dvdP2=dvdz*Xr[0];
    dvdP5=dvdz*Xr[1];
    dvdP8=dvdz*Xr[2];
        
    /* Jacobian */
        
    /* motion parameters for u */
    J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
    J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
    J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
    J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
    J[4]=dudx;       //t1
    J[5]=0.;           //t2
    J[6]=dudz;           //t3
    /* structure parameters for u */
    J[7]=dudxr0*dxr0dx0+dudxr1*dxr1dx0+dudxr2*dxr2dx0;
    J[8]=dudxr0*dxr0dx1+dudxr1*dxr1dx1+dudxr2*dxr2dx1;
    J[9]=dudxr0*dxr0dx2+dudxr1*dxr1dx2+dudxr2*dxr2dx2;
    
    /* motion parameters for v */
    J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
    J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
    J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
    J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
    J[D+4]=0.;           /* t1 */
    J[D+5]=dvdy;       /* t2 */
    J[D+6]=dvdz;           /* t3 */
    /* structure parameters for v */
    J[D+7]=dvdxr0*dxr0dx0+dvdxr1*dxr1dx0+dvdxr2*dxr2dx0;
    J[D+8]=dvdxr0*dxr0dx1+dvdxr1*dxr1dx1+dvdxr2*dxr2dx1;
    J[D+9]=dvdxr0*dxr0dx2+dvdxr1*dxr1dx2+dvdxr2*dxr2dx2;
}

void proj3D_fun_tnorm(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a,tnorm;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int D=10;


    norm=1./(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */
    tnorm=1./sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2]);
    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0]*tnorm;
    P[10]=t[1]*tnorm;
    P[11]=t[2]*tnorm;

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];

    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*X[3*ind]+P[3]*X[1+3*ind]+P[6]*X[2+3*ind]+P[9];
    y=P[1]*X[3*ind]+P[4]*X[1+3*ind]+P[7]*X[2+3*ind]+P[10];
    z=P[2]*X[3*ind]+P[5]*X[1+3*ind]+P[8]*X[2+3*ind]+P[11];
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;   
    if(z<0)
    {
      /* need to handle this properly 
      i.e e=0-z ? and initialising*/
      e[0]=th;
      e[1]=th;
    }
    /* help variables ? necessary */
    z2=z*z;
    f1=-p[0]*x/z2;
    f2=-p[0]*y/z2;
    dudP0=p[0]*X[3*ind]/z;
    dudP3=p[0]*X[1+3*ind]/z;
    dudP6=p[0]*X[2+3*ind]/z;
    dudP2=f1*X[3*ind];
    dudP5=f1*X[1+3*ind];
    dudP8=f1*X[2+3*ind];
        
    dvdP1=p[0]*X[3*ind]/z;
    dvdP4=p[0]*X[1+3*ind]/z;
    dvdP7=p[0]*X[2+3*ind]/z;
    dvdP2=f2*X[3*ind];
    dvdP5=f2*X[1+3*ind];
    dvdP8=f2*X[2+3*ind];
        
    /* Jacobian */
        /*
        J[0]=x/z;
        J[1]=1.;
        J[2]=0.;
         */
        
    /* motion parameters for u */
    J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
    J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
    J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
    J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
    J[4]=p[0]*tnorm/z-p[0]*t[0]*t[0]*tnorm*tnorm/z;       //t1
    J[5]=0.;           //t2
    J[6]=f1*tnorm;           //t3
    /* structure parameters for u */
    J[7]=p[0]*P[0]/z+f1*P[2];
    J[8]=p[0]*P[3]/z+f1*P[5];
    J[9]=p[0]*P[6]/z+f1*P[8];
    /*
        J[D]=y/z;
        J[D+1]=0.;
        J[D+2]=1.;
          
        */
    /* motion parameters for v */
    J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
    J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
    J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
    J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
    J[D+4]=0.;           /* t1 */
    J[D+5]=p[0]*tnorm/z;       /* t2 */
    J[D+6]=f2*tnorm;           /* t3 */
    /* structure parameters for v */
    J[D+7]=p[0]*P[1]/z+f2*P[2];
    J[D+8]=p[0]*P[4]/z+f2*P[5];
    J[D+9]=p[0]*P[7]/z+f2*P[8];
        
 
 
}

void proj3D_funp(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a;
    double dudP0,dudP2,dudP3,dudP5,dudP6,dudP8,dvdP1,dvdP2,dvdP4,dvdP5,dvdP7,dvdP8,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];
    int D=7;
    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*P[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*P[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*P[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*P[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*P[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*P[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*P[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*P[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*P[8];
    
    dRdq1[0]= q[1]*norm*2-a*q[1]*P[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*P[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*P[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*P[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*P[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*P[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*P[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*P[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*P[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*P[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*P[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*P[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*P[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*P[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*P[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*P[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*P[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*P[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*P[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*P[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*P[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*P[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*P[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*P[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*P[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*P[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*P[8];
    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*X[3*ind]+P[3]*X[1+3*ind]+P[6]*X[2+3*ind]+P[9];
    y=P[1]*X[3*ind]+P[4]*X[1+3*ind]+P[7]*X[2+3*ind]+P[10];
    z=P[2]*X[3*ind]+P[5]*X[1+3*ind]+P[8]*X[2+3*ind]+P[11];
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;
    if(z<0)
    {
      e[0]=th;
      e[1]=th;
    }
    /* help variables ? necessary */
    z2=z*z;
    f1=-p[0]*x/z2;
    f2=-p[0]*y/z2;
    dudP0=p[0]*X[3*ind]/z;
    dudP3=p[0]*X[1+3*ind]/z;
    dudP6=p[0]*X[2+3*ind]/z;
    dudP2=f1*X[3*ind];
    dudP5=f1*X[1+3*ind];
    dudP8=f1*X[2+3*ind];
        
    dvdP1=p[0]*X[3*ind]/z;
    dvdP4=p[0]*X[1+3*ind]/z;
    dvdP7=p[0]*X[2+3*ind]/z;
    dvdP2=f2*X[3*ind];
    dvdP5=f2*X[1+3*ind];
    dvdP8=f2*X[2+3*ind];
        

    /* motion parameters for u */
    J[0]=dudP0*dRdq0[0]+dudP2*dRdq0[2]+dudP3*dRdq0[3]+dudP5*dRdq0[5]+dudP6*dRdq0[6]+dudP8*dRdq0[8];
    J[1]=dudP0*dRdq1[0]+dudP2*dRdq1[2]+dudP3*dRdq1[3]+dudP5*dRdq1[5]+dudP6*dRdq1[6]+dudP8*dRdq1[8];
    J[2]=dudP0*dRdq2[0]+dudP2*dRdq2[2]+dudP3*dRdq2[3]+dudP5*dRdq2[5]+dudP6*dRdq2[6]+dudP8*dRdq2[8];
    J[3]=dudP0*dRdq3[0]+dudP2*dRdq3[2]+dudP3*dRdq3[3]+dudP5*dRdq3[5]+dudP6*dRdq3[6]+dudP8*dRdq3[8];
    J[4]=p[0]/z;       //t1
    J[5]=0.;           //t2
    J[6]=f1;           //t3

    /* motion parameters for v */
    J[D]=dvdP1*dRdq0[1]+dvdP2*dRdq0[2]+dvdP4*dRdq0[4]+dvdP5*dRdq0[5]+dvdP7*dRdq0[7]+dvdP8*dRdq0[8];
    J[D+1]=dvdP1*dRdq1[1]+dvdP2*dRdq1[2]+dvdP4*dRdq1[4]+dvdP5*dRdq1[5]+dvdP7*dRdq1[7]+dvdP8*dRdq1[8];
    J[D+2]=dvdP1*dRdq2[1]+dvdP2*dRdq2[2]+dvdP4*dRdq2[4]+dvdP5*dRdq2[5]+dvdP7*dRdq2[7]+dvdP8*dRdq2[8];
    J[D+3]=dvdP1*dRdq3[1]+dvdP2*dRdq3[2]+dvdP4*dRdq3[4]+dvdP5*dRdq3[5]+dvdP7*dRdq3[7]+dvdP8*dRdq3[8];
    J[D+4]=0.;           /* t1 */
    J[D+5]=p[0]/z;       /* t2 */
    J[D+6]=f2;           /* t3 */

        
 
 
}
void proj3D_funs(
		double *e,
		double *J,
		double *q,
		double *t,
		double *p,
		double *pts,
		double *X,
		int ind,
		double th
		)
{
        double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;
	
	
	
	norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
	q1t2=q[0]*q[0];
	q2t2=q[1]*q[1];
	q3t2=q[2]*q[2];
	q4t2=q[3]*q[3];
	q1q2=q[0]*q[1];
	q2q3=q[1]*q[2];
	q3q4=q[2]*q[3];
	q1q4=q[0]*q[3];
	q2q4=q[1]*q[3];
	q1q3=q[0]*q[2];
	/* building Euclidian transformation matrix */

	P[0]=(q1t2+q2t2-q3t2-q4t2);
	P[1]=2*(q2q3+q1q4);
	P[2]=2*(q2q4-q1q3);
	P[3]=2*(q2q3-q1q4);
	P[4]=(q1t2-q2t2+q3t2-q4t2);
	P[5]=2*(q3q4+q1q2);
	P[6]=2*(q2q4+q1q3);
	P[7]=2*(q3q4-q1q2);
	P[8]=(q1t2-q2t2-q3t2+q4t2);
	P[9]=t[0];
	P[10]=t[1];
	P[11]=t[2];

	P[0]*=norm;
	P[1]*=norm;
	P[2]*=norm;
	P[3]*=norm;
	P[4]*=norm;
	P[5]*=norm;
	P[6]*=norm;
	P[7]*=norm;
	P[8]*=norm;

    /* projecting object 3D-coordinates to camera frame */
	x=P[0]*X[3*ind]+P[3]*X[1+3*ind]+P[6]*X[2+3*ind]+P[9];
	y=P[1]*X[3*ind]+P[4]*X[1+3*ind]+P[7]*X[2+3*ind]+P[10];
	z=P[2]*X[3*ind]+P[5]*X[1+3*ind]+P[8]*X[2+3*ind]+P[11];
    if(z==0)
	    z=-0.000000001; /* fixme */
    /*residuals */
	e[0]=pts[0]-p[1]-p[0]*x/z;
	e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
		e[0]=th;
    if(e[0]<-th)
		e[0]=-th;
	if(e[1]>th)
		e[1]=th;
    if(e[1]<-th)
		e[1]=-th;
    if(z<0)
    {
      e[0]=th;
      e[1]=th;
    }
    /* help variables ? necessary */
	z2=z*z;
	f1=-p[0]*x/z2;
	f2=-p[0]*y/z2;
        
    /* Jacobian */
    /* structure parameters for u */
	J[0]=p[0]*P[0]/z+f1*P[2];
	J[1]=p[0]*P[3]/z+f1*P[5];
	J[2]=p[0]*P[6]/z+f1*P[8];

    /* structure parameters for v */
	J[3]=p[0]*P[1]/z+f2*P[2];
	J[4]=p[0]*P[4]/z+f2*P[5];
	J[5]=p[0]*P[7]/z+f2*P[8];
        
        /*
        J[D+1]=x/z;
        J[D+2]=1;
        J[L+D+1]=y/z;
        J[L+D+2]=1;
        */
 
}
void proj3D_funs1(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,f1,f2,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;
    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];

    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;

    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*(X[3*ind]+P[9])+P[3]*(X[1+3*ind]+P[10])+P[6]*(X[2+3*ind]+P[11]);
    y=P[1]*(X[3*ind]+P[9])+P[4]*(X[1+3*ind]+P[10])+P[7]*(X[2+3*ind]+P[11]);
    z=P[2]*(X[3*ind]+P[9])+P[5]*(X[1+3*ind]+P[10])+P[8]*(X[2+3*ind]+P[11]);
    if(z==0)
        z=-0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;   
    /* help variables ? necessary */
    z2=z*z;
    f1=-p[0]*x/z2;
    f2=-p[0]*y/z2;

    /* structure parameters for u */
    J[0]=p[0]*P[0]/z+f1*P[2];
    J[1]=p[0]*P[3]/z+f1*P[5];
    J[2]=p[0]*P[6]/z+f1*P[8];
    

    /* structure parameters for v */
    J[3]=p[0]*P[1]/z+f2*P[2];
    J[4]=p[0]*P[4]/z+f2*P[5];
    J[5]=p[0]*P[7]/z+f2*P[8];
}
void stereo_proj3D_funs(
        double *e,
        double *J,
        double *q,
        double *t,
        double *p,
        double *R,
        double *t0,               
        double *pts,
        double *X,
        int ind,
        double th
        )
{
    double P[12],z,x,y,z2,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,Xr[3];

    double dvdz,dvdy,dudz,dudx,dudxr0,dudxr1,dudxr2,dvdxr0,dvdxr1,dvdxr2,dxr0dx0,dxr0dx1,dxr0dx2,dxr1dx0,dxr1dx1,dxr1dx2,dxr2dx0,dxr2dx1,dxr2dx2;

    
    
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building Euclidian transformation matrix */

    P[0]=(q1t2+q2t2-q3t2-q4t2);
    P[1]=2*(q2q3+q1q4);
    P[2]=2*(q2q4-q1q3);
    P[3]=2*(q2q3-q1q4);
    P[4]=(q1t2-q2t2+q3t2-q4t2);
    P[5]=2*(q3q4+q1q2);
    P[6]=2*(q2q4+q1q3);
    P[7]=2*(q3q4-q1q2);
    P[8]=(q1t2-q2t2-q3t2+q4t2);
    P[9]=t[0];
    P[10]=t[1];
    P[11]=t[2];


    
    P[0]*=norm;
    P[1]*=norm;
    P[2]*=norm;
    P[3]*=norm;
    P[4]*=norm;
    P[5]*=norm;
    P[6]*=norm;
    P[7]*=norm;
    P[8]*=norm;
    Xr[0]=R[0]*X[3*ind]+R[3]*X[1+3*ind]+R[6]*X[2+3*ind]+t0[0];
    Xr[1]=R[1]*X[3*ind]+R[4]*X[1+3*ind]+R[7]*X[2+3*ind]+t0[1];
    Xr[2]=R[2]*X[3*ind]+R[5]*X[1+3*ind]+R[8]*X[2+3*ind]+t0[2];
    /* projecting object 3D-coordinates to camera frame */
    x=P[0]*Xr[0]+P[3]*Xr[1]+P[6]*Xr[2]+P[9];
    y=P[1]*Xr[0]+P[4]*Xr[1]+P[7]*Xr[2]+P[10];
    z=P[2]*Xr[0]+P[5]*Xr[1]+P[8]*Xr[2]+P[11];
    if(z==0)
        z=0.000000001; /* fixme */
    /*residuals */
    e[0]=pts[0]-p[1]-p[0]*x/z;
    e[1]=pts[1]-p[2]-p[0]*y/z;
    if(e[0]>th)
        e[0]=th;
    if(e[0]<-th)
        e[0]=-th;
    if(e[1]>th)
        e[1]=th;
    if(e[1]<-th)
        e[1]=-th;   
    /* help variables ? necessary */

    z2=z*z;
    dudxr0=p[0]*(P[0]/z-x*P[2]/z2);
    dudxr1=p[0]*(P[3]/z-x*P[5]/z2);
    dudxr2=p[0]*(P[6]/z-x*P[8]/z2);
    dvdxr0=p[0]*(P[1]/z-y*P[2]/z2);
    dvdxr1=p[0]*(P[4]/z-y*P[5]/z2);
    dvdxr2=p[0]*(P[7]/z-y*P[8]/z2);
    dxr0dx0=R[0];
    dxr0dx1=R[3];
    dxr0dx2=R[6];
    dxr1dx0=R[1];
    dxr1dx1=R[4];
    dxr1dx2=R[7];
    dxr2dx0=R[2];
    dxr2dx1=R[5];
    dxr2dx2=R[8];

    dudz=-p[0]*x/z2;
    dudx=p[0]/z;
    dvdz=-p[0]*y/z2;
    dvdy=p[0]/z;
    

        
    /* Jacobian */
        

    /* structure parameters for u */
    J[0]=dudxr0*dxr0dx0+dudxr1*dxr1dx0+dudxr2*dxr2dx0;
    J[1]=dudxr0*dxr0dx1+dudxr1*dxr1dx1+dudxr2*dxr2dx1;
    J[2]=dudxr0*dxr0dx2+dudxr1*dxr1dx2+dudxr2*dxr2dx2;
    

    /* structure parameters for v */
    J[3]=dvdxr0*dxr0dx0+dvdxr1*dxr1dx0+dvdxr2*dxr2dx0;
    J[4]=dvdxr0*dxr0dx1+dvdxr1*dxr1dx1+dvdxr2*dxr2dx1;
    J[5]=dvdxr0*dxr0dx2+dvdxr1*dxr1dx2+dvdxr2*dxr2dx2;
}
void calc_CC(
		double *vv,
		double *vx,
		double *xx,
		double *C,
        double P,
		int Nv,
		int Nx,
		int p,
		int ind,
		int frame)
{
	
	int i,j,D,iN,base,basep;
	D=7;
    
	base=ind*p; /* depends number_of_pose parameters and number_of_poses */
	basep=frame; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */
	/* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE
        Nv=D*num_frames;
	Nx=N-Nv;*/
 
	/*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
	for(i=0;i<D;i++)
	{
		iN=(i+basep)*Nv;
		for(j=0;j<D;j++)
		{
			vv[j+basep+iN]+=C[i]*C[j]*P;
		}
	}
	/*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
	/* 
	JJ(i,j)=J(i)*J(j);
	CC(i*p+i-j)=JJ(i,j);*/
	
	for(i=0;i<p;i++)
	{
		for(j=i;j<p;j++)
		{
			xx[j-i+(base+i)*p]+=C[i+D]*C[j+D]*P;
		}
	}
	
	/*vx*/
	for(i=0;i<D;i++)
	{
		for(j=0;j<p;j++)
		{
			vx[j+base+(i+basep)*Nx]+=C[i]*C[j+D]*P;
		}
	} 
	    
}

void calc3Dlink_CC(
        double *CC,
        int N,
        double *C,
        double *S,
        int ind,
        int D,
        int D2
)
{   
    int k,i,j,iN;

    for(k=0;k<D;k++)
    {
        for(i=0;i<D2;i++)
        {
            iN=(i+ind)*N;
            for(j=0;j<D2;j++)
            {
                CC[j+ind+iN]+=C[i+k*D2]*C[j+k*D2]*S[k];
       
            }
        }
    }
    


}
void calc3Dlink_Ce(
        double *Ce,
        double *C,
        double *P,
        double *e,
        int ind,
        int D,
        int D2
)
{
    int i,k;

    for(k=0;k<D;k++)
    {
        for(i=0;i<D2;i++)
        {
            Ce[i+ind]+=C[i+k*D2]*e[k]*P[k];
        }
    }

}

void calc_CCd(
		double *vv,
		double *C,
		double P,
        int D,
		int p,
		int ind)
{

    int i,j,iN,base;
    
    base=ind*D;
    for(i=0;i<p;i++)
    {
        iN=base+i*D;
        for(j=0;j<p;j++)
        {
            vv[j+iN]+=C[i]*C[j]*P;
        }
    }
}
void calc_CCb(
		double *xx,
		double *C,
        double P,
		int N,
		int p,
		int base
		)
{
    int i,j;

    
        /* 
	JJ(i,j)=J(i)*J(j);
	CC(i*p+i-j)=JJ(i,j);*/
	
    for(i=0;i<N;i++)
    {
        for(j=i;j<N;j++)
        {
            xx[j-i+(base+i)*p]+=C[i]*C[j]*P;
        }
    }
}
void calc_CCs(
		double *xx,
		double *C,
        double P,
		int p,
		int ind
		)
{
	
	int i,j,base;
	
    
	base=ind*p; /* depends number_of_pose parameters and number_of_poses */
	/* redundancy here, slow loops */
	/* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE*/
    
	/*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
	/*base=(ind-1)*3+D; */
		/* 
	JJ(i,j)=J(i)*J(j);
	CC(i*p+i-j)=JJ(i,j);*/
	
	for(i=0;i<p;i++)
	{
		for(j=i;j<p;j++)
		{
			xx[j-i+(base+i)*p]+=C[i]*C[j]*P;
		}
	}
	
/*
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			CC[j+base+(base+i)*N]+=C[i]*C[j];
		}
	}*/
}
void calc_CCs3(
        double *xx,
        double *C,
        double P,
        int ind
        )
{
    
    int base,basep;
    
    
    base=ind*3;
    basep=base*3;
    /* depends number_of_pose parameters and number_of_poses */
    /* redundancy here, slow loops */
    /* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE*/
    
    /*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
    /*base=(ind-1)*3+D; */
        /* 
    JJ(i,j)=J(i)*J(j);
    CC(i*p+i-j)=JJ(i,j);*/

    xx[basep]+=C[0]*C[0]*P;
    xx[1+basep]+=C[0]*C[1]*P;
    xx[2+basep]+=C[0]*C[2]*P;
    xx[basep+3]+=C[1]*C[1]*P;
    xx[1+basep+3]+=C[1]*C[2]*P;
    xx[basep+6]+=C[2]*C[2]*P;

}
void calc_CC3(
        double *vv,
        double *vx,
        double *xx,
        double *C,
        double P,
        int Nv,
        int Nx,
        int ind,
        int frame)
{
    
    int i,D,iN,base,basep,basepp;
    D=7;
    
    base=ind*3; /* depends number_of_pose parameters and number_of_poses */
    basep=frame;
    basepp=base*3;/* depends number_of_pose parameters and current_frame */
    /* redundancy here, slow loops */
    /* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE
        Nv=D*num_frames;
    Nx=N-Nv;*/
 
    /*vv C(1:D)=vehicle states, C(1:D)*C(1:D)' */
    for(i=0;i<D;i++)
    {
        iN=(i+basep)*Nv;
        vv[0+basep+iN]+=C[i]*C[0]*P;
        vv[1+basep+iN]+=C[i]*C[1]*P;
        vv[2+basep+iN]+=C[i]*C[2]*P;
        vv[3+basep+iN]+=C[i]*C[3]*P;
        vv[4+basep+iN]+=C[i]*C[4]*P;
        vv[5+basep+iN]+=C[i]*C[5]*P;
        vv[6+basep+iN]+=C[i]*C[6]*P;
    }
    /*xx   C(D:D+3),CC(base:base+3,base:base+3)=C(base:base+3)*C(base:base+3)'*/
    /* 
    JJ(i,j)=J(i)*J(j);
    CC(i*p+i-j)=JJ(i,j);*/
    xx[basepp]+=C[0]*C[0]*P;
    xx[1+basepp]+=C[0]*C[1]*P;
    xx[2+basepp]+=C[0]*C[2]*P;
    xx[basepp+3]+=C[1]*C[1]*P;
    xx[1+basepp+3]+=C[1]*C[2]*P;
    xx[basepp+6]+=C[2]*C[2]*P;

    
    /*vx*/
    for(i=0;i<D;i++)
    {
            vx[base+(i+basep)*Nx]+=C[i]*C[D]*P;
            vx[1+base+(i+basep)*Nx]+=C[i]*C[1+D]*P;
            vx[2+base+(i+basep)*Nx]+=C[i]*C[2+D]*P;
    } 
        
}
void calc_Ce(
		double *Ce,
		double *C,
		double *e,
        double P,
		int ind,
		int frame,
		int num_frames)
{
	
	int j,base,basep,D=7;
	

	base=ind*3+num_frames; /* depends number_of_pose parameters and number_of_poses */
	basep=frame; /* depends number_of_pose parameters and current_frame */
	/* redundancy here, slow loops */
	/* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE*/
	
	/*vx*/
    
	for(j=0;j<D;j++)
	{
		Ce[basep+j]+=C[j]*e[0]*P;
	}
	for(j=0;j<3;j++)
	{
		Ce[base+j]+=C[j+D]*e[0]*P;
	}
}
void calc_Ced(
        double *Ce,
        double *C,
        double *e,
        double P,
        int base,
        int D)
{
    
    int j;
   
    for(j=0;j<D;j++)
    {
        Ce[base+j]+=C[j]*e[0]*P;
    }
   
}
void calc_Ces(
		double *Ce,
		double *C,
		double *e,
		double P,
        int ind,
		int num_frames)
{
	
	int j,base;

	base=ind*3+num_frames; /* depends number_of_pose parameters and number_of_poses */
	/* redundancy here, slow loops */
	/* don't blame it on sunshine, don't blame it on red wine,don't blame it on good time,  blame it on the BOOGIE*/
	/*vx*/
    
	for(j=0;j<3;j++)
	{
		Ce[base+j]+=C[j]*e[0]*P;
	}
}


void slam3D_batch_er1(
        double *p,   
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double *ssd,
        double *er,
        double *depth
        )
{
    double e[2],q[4],t[3],de[2];
    int j,k,indi,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;



    for(j=0;j<N;j++) /* for each measurement */
    {   
        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
         
        /*calculate Jacobian and residual */

            proj3D_fun_er1(e,de,q,t,p,z+j*2,Xpri+Nv,indi);

        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        
            proj3D_fun_er1(e,de,q,t,p,z+j*2,Xpri+Nv,indi);
  
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
 //           printf("er %f %f depth %f\n",e[0],e[1],de[0]);
            (*ssd)+=er[j];
        }
    }
}

void slam3D_batch_er(
        double *p,   
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double *ssd,
        double *er,
        double *depth
        )
{
    double e[2],q[4],t[3],de[2];
    int j,k,indi,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;



    for(j=0;j<N;j++) /* for each measurement */
    {   
        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
         
        /*calculate Jacobian and residual */

            proj3D_fun_er(e,de,q,t,p,z+j*2,Xpri+Nv,indi);

        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        
            proj3D_fun_er(e,de,q,t,p,z+j*2,Xpri+Nv,indi);
  
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
 //           printf("er %f %f depth %f\n",e[0],e[1],de[0]);
            (*ssd)+=er[j];
        }
    }
}

void slam3D_stereo_er(
        double *pl,
        double *pr,
        double *stereo_par,      
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        int *indc,
        double *z,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double *ssd,
        double *er,
        double *depth
        )
{
    double e[2],q[4],t[3],*t0,R0[9],de[2];
    int j,k,indi,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;


    t0=stereo_par+4;
    
 //   printf("debug:q0 %f %f %f %f t0 %f %f %f \n",stereo_par[0],stereo_par[1],stereo_par[2],stereo_par[3],t0[0],t0[1],t0[2]);
    quater_2_rot(R0,stereo_par);
  //  printf("debug:R0 %f %f %f %f %f %f %f %f %f \n",R0[0],R0[1],R0[2],R0[3],R0[4],R0[5],R0[6],R0[7],R0[8]);
    for(j=0;j<N;j++) /* for each measurement */
    {   
      

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
         

        /*calculate Jacobian and residual */
            if(indc[j]==0)
            {
                proj3D_fun_er(e,de,q,t,pl,z+j*2,Xpri+Nv,indi);
            }
            else if(indc[j]==1)
            {
                stereo_proj3D_fun_er(e,de,q,t,pr,R0,t0,z+j*2,Xpri+Nv,indi);
            }
 
        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        
            if(indc[j]==0)
            {
                proj3D_fun_er(e,de,q,t,pl,z+j*2,Xpri+Nv,indi);
            }
            else if(indc[j]==1)
            {
                 stereo_proj3D_fun_er(e,de,q,t,pr,R0,t0,z+j*2,Xpri+Nv,indi);
            } 
  
            er[j]=e[0]*e[0]+e[1]*e[1];
            depth[j]=de[0];
 //           printf("er %f %f depth %f\n",e[0],e[1],de[0]);
            (*ssd)+=er[j];
          
        }
    }
}


void slam3D_iter_stereo_batch(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *pl,
        double *pr,
        double *stereo_par,      
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        int *indc,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3],*t0,R0[9];
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;

    t0=stereo_par+4;
    
 //   printf("debug:q0 %f %f %f %f t0 %f %f %f \n",stereo_par[0],stereo_par[1],stereo_par[2],stereo_par[3],t0[0],t0[1],t0[2]);
    quater_2_rot(R0,stereo_par);
  //  printf("debug:R0 %f %f %f %f %f %f %f %f %f \n",R0[0],R0[1],R0[2],R0[3],R0[4],R0[5],R0[6],R0[7],R0[8]);
    for(j=0;j<N;j++) /* for each measurement */
    {   
      

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
          /*  if(start_frame==1)
            {
                if(frame==0)
                {
                    tnorm=1./sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2]);
                    t[0]=t[0]*tnorm;
                    t[1]=t[1]*tnorm;
                    t[2]=t[2]*tnorm;
                    Xpri[frame*7+4]=t[0];
                    Xpri[frame*7+5]=t[1];
                    Xpri[frame*7+6]=t[2];
                }
            }*/
 

        /*calculate Jacobian and residual */
            if(indc[j]==0)
            {
                proj3D_fun(e,C,q,t,pl,z+j*2,Xpri+Nv,indi,th);
            }
            else if(indc[j]==1)
            {
                stereo_proj3D_fun(e,C,q,t,pr,R0,t0,z+j*2,Xpri+Nv,indi,th);
            }
 /*           if(j<1)
            {
                printf("debug:C0 %f %f %f %f %f %f %f %f %f %f \n",C[0],C[1],C[2],C[3],C[4],C[5],C[6],C[7],C[8],C[9]);
                printf("debug:C1 %f %f %f %f %f %f %f %f %f %f \n",C[0+10],C[1+10],C[2+10],C[3+10],C[4+10],C[5+10],C[6+10],C[7+10],C[8+10],C[9+10]);
                printf("debug:q %f %f %f %f t %f %f %f \n",q[0],q[1],q[2],q[3],t[0],t[1],t[2]);
                printf("debug:j %d ind %d indf %d indc %d e0 %f e1 %f\n",j,ind[j],indf[j],indc[j],e[0],e[1]);
                printf("debug:z %f %f  X %f %f %f \n",z[2*j],z[2*j+1],Xpri[Nv+indi*3],Xpri[Nv+indi*3+1],Xpri[Nv+indi*3+2]);
            }*/
        /*calculate covariance matrix multiplications */

            calc_Ce(Ce, C, e, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C, 1., Nv, Nx, 3, indi, 7*frame);
            calc_Ce(Ce, C+10, e+1, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C+10, 1., Nv,Nx, 3, indi, 7*frame);

        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        
            if(indc[j]==0)
            {
                proj3D_funs(e,C,q,t,pl,z+2*j,Xpri+Nv,indi,th);
            }
            else if(indc[j]==1)
            {
                 stereo_proj3D_funs(e,C,q,t,pr,R0,t0,z+j*2,Xpri+Nv,indi,th);
            } 
   /*         if(j<1)
            {
                printf("debug:C0 %f %f %f\n",C[0],C[1],C[2]);
                printf("debug:C1 %f %f %f \n",C[3],C[4],C[5]);
                printf("debug:q %f %f %f %f t %f %f %f \n",q[0],q[1],q[2],q[3],t[0],t[1],t[2]);
                printf("debug:j %d ind %d indf %d indc %d e0 %f e1 %f\n",j,ind[j],indf[j],indc[j],e[0],e[1]);
                printf("debug:z %f %f  X %f %f %f \n",z[2*j],z[2*j+1],Xpri[Nv+indi*3],Xpri[Nv+indi*3+1],Xpri[Nv+indi*3+2]);
            }*/
    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, 1., indi, Nv);
            calc_CCs(xx, C, 1., 3, indi);
            calc_Ces(Ce, C+3, e+1, 1., indi, Nv);
            calc_CCs(xx, C+3, 1., 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
          
        }
    }

        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);
            

            slam_vx_icb(icb,vx,R,Nv,Nx,Nv);
 /*           
            for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];
   */         
            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}
void slam3D_iter_struc_stereo_batch(
        double *L,
        double *g,
        double *pl,
        double *pr,
        double *stereo_par,      
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indc,
        double *z,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3],*t0,R0[9];
    int j,k,indi,ret;
 


    k=0;
    (*ssd)=0;

    for(j=0;j<D*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;

    t0=stereo_par+4;
    
 //   printf("debug:q0 %f %f %f %f t0 %f %f %f \n",stereo_par[0],stereo_par[1],stereo_par[2],stereo_par[3],t0[0],t0[1],t0[2]);
    quater_2_rot(R0,stereo_par);
//    printf("debug:R0 %f %f %f %f %f %f %f %f %f \n",R0[0],R0[1],R0[2],R0[3],R0[4],R0[5],R0[6],R0[7],R0[8]);
    q[0]=X0[0];
    q[1]=X0[1];
    q[2]=X0[2];
    q[3]=X0[3];
    t[0]=X0[4];
    t[1]=X0[5];
    t[2]=X0[6];
    for(j=0;j<N;j++) /* for each measurement */
    {   
     
            indi=ind[j];

            indi--;
                 
        
            if(indc[j]==0)
            {
                proj3D_funs(e,C,q,t,pl,z+2*j,Xpri,indi,th);
            }
            else if(indc[j]==1)
            {
                 stereo_proj3D_funs(e,C,q,t,pr,R0,t0,z+j*2,Xpri,indi,th);
            } 
   /*         if(j<1)
            {
                printf("debug:C0 %f %f %f\n",C[0],C[1],C[2]);
                printf("debug:C1 %f %f %f \n",C[3],C[4],C[5]);
                printf("debug:q %f %f %f %f t %f %f %f \n",q[0],q[1],q[2],q[3],t[0],t[1],t[2]);
                printf("debug:j %d ind %d indf %d indc %d e0 %f e1 %f\n",j,ind[j],indf[j],indc[j],e[0],e[1]);
                printf("debug:z %f %f  X %f %f %f \n",z[2*j],z[2*j+1],Xpri[Nv+indi*3],Xpri[Nv+indi*3+1],Xpri[Nv+indi*3+2]);
            }*/
    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, 1., indi, 0);
            calc_CCs(xx, C, 1., 3, indi);
            calc_Ces(Ce, C+3, e+1, 1., indi, 0);
            calc_CCs(xx, C+3, 1., 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];      
     
    }

        /* should check if previous move was a success before next */

        do
        {

            for(j=0;j<D*3;j++)
                L[j]=xx[j];
            for(j=0;j<D;j++)
                L[j*3]+=lambda;

            /* go */
            ret=choldcb(L, L, D,3);        
            cholslb(L,Ce,g,D,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];


}

void slam3D_iter_batch(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3];
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;


    for(j=0;j<N;j++) /* for each measurement */
    {   
      

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
          /*  if(start_frame==1)
            {
                if(frame==0)
                {
                    tnorm=1./sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2]);
                    t[0]=t[0]*tnorm;
                    t[1]=t[1]*tnorm;
                    t[2]=t[2]*tnorm;
                    Xpri[frame*7+4]=t[0];
                    Xpri[frame*7+5]=t[1];
                    Xpri[frame*7+6]=t[2];
                }
            }*/
 

        /*calculate Jacobian and residual */

            proj3D_fun(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);

        /*calculate covariance matrix multiplications */

            calc_Ce(Ce, C, e, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C, 1., Nv, Nx, 3, indi, 7*frame);
            calc_Ce(Ce, C+10, e+1, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C+10, 1., Nv,Nx, 3, indi, 7*frame);

        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        
            proj3D_funs(e,C,q,t,p,z+2*j,Xpri+Nv,indi,th);

    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, 1., indi, Nv);
            calc_CCs(xx, C, 1., 3, indi);
            calc_Ces(Ce, C+3, e+1, 1., indi, Nv);
            calc_CCs(xx, C+3, 1., 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];  
        }
    }

        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);		
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);
            

            slam_vx_icb(icb,vx,R,Nv,Nx,Nv);
 /*           
            for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];
   */         
            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}

void slam3D_iter_batch1(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3];
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;


    for(j=0;j<N;j++) /* for each measurement */
    {   
      

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
          /*  if(start_frame==1)
            {
                if(frame==0)
                {
                    tnorm=1./sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2]);
                    t[0]=t[0]*tnorm;
                    t[1]=t[1]*tnorm;
                    t[2]=t[2]*tnorm;
                    Xpri[frame*7+4]=t[0];
                    Xpri[frame*7+5]=t[1];
                    Xpri[frame*7+6]=t[2];
                }
            }*/
 

        /*calculate Jacobian and residual */

            proj3D_fun1(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);

        /*calculate covariance matrix multiplications */

            calc_Ce(Ce, C, e, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C, 1., Nv, Nx, 3, indi, 7*frame);
            calc_Ce(Ce, C+10, e+1, 1., indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C+10, 1., Nv,Nx, 3, indi, 7*frame);

        /* measurement error */
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        

            proj3D_funs1(e,C,q,t,p,z+2*j,Xpri+Nv,indi,th);

    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, 1., indi, Nv);
            calc_CCs(xx, C, 1., 3, indi);
            calc_Ces(Ce, C+3, e+1, 1., indi, Nv);
            calc_CCs(xx, C+3, 1., 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
          
        }
    }

        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);
            

            slam_vx_icb(icb,vx,R,Nv,Nx,Nv);
 /*           
            for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];
   */         
            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}

void slam3D_ini(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3];
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;

    while(((indf[k])==0)&&k<N)
    {
        indi=ind[k];
        indi--;
        q[0]=X0[0];
        q[1]=X0[1];
        q[2]=X0[2];
        q[3]=X0[3];
        t[0]=X0[4];
        t[1]=X0[5];
        t[2]=X0[6];

        proj3D_funs(e,C,q,t,p,z+2*k,Xpri+Nv,indi,th);
/*  printf("PC:");
    for(i=0;i<20;i++)
        printf("%lf  ",C[i]);
    printf("\n");
    */
    /*calculate sparse covariance matrix multiplications */
        calc_Ces(Ce, C, e, 1., indi, Nv);
        calc_CCs(xx, C, 1., 3, indi);
        calc_Ces(Ce, C+3, e+1, 1., indi, Nv);
        calc_CCs(xx, C+3, 1., 3, indi);
        er[k]=e[0]*e[0]+e[1]*e[1];
        (*ssd)+=er[k];
        k++;
    }

    for(j=k;j<N;j++) /* for each measurement */
    {

        /* indi is the index to the landmark that corresponds to measurement i*/
        indi=ind[j];
        frame=indf[j];
        indi--;
        frame--;
        q[0]=Xpri[frame*7];
        q[1]=Xpri[frame*7+1];
        q[2]=Xpri[frame*7+2];
        q[3]=Xpri[frame*7+3];
        t[0]=Xpri[frame*7+4];
        t[1]=Xpri[frame*7+5];
        t[2]=Xpri[frame*7+6];


        /*calculate Jacobian and residual */

        proj3D_fun(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);
/*  printf("JC:");
    for(i=0;i<20;i++)
        printf("%lf  ",C[i]);
    printf("\n");*/
        /*calculate covariance matrix multiplications */

        calc_Ce(Ce, C, e, 1., indi, 7*frame, Nv);
        calc_CC(vv,vx,xx, C, 1., Nv, Nx, 3, indi, 7*frame);
        calc_Ce(Ce, C+10, e+1, 1., indi, 7*frame, Nv);
        calc_CC(vv,vx,xx, C+10, 1., Nv,Nx, 3, indi, 7*frame);

        /* measurement error */
        er[j]=e[0]*e[0]+e[1]*e[1];
        (*ssd)+=er[j];
    }

        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);

            for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];

            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}



void calc_3D_link0(
            double *e,
            double *J,
            double *X0,
            double *X,
            double *t,   
            int j
              )
{
    double ssum,rhat,dx,dy,dz;

    dx=X[4]-X0[4];
    dy=X[5]-X0[5];
    dz=X[6]-X0[6];
    
    ssum=dx*dx+dy*dy+dz*dz;
    rhat=sqrt(ssum);
    e[0]=t[0]-rhat;
    
    /*Jacobian*/
    J[0]=dx/rhat;
    J[1]=dy/rhat;
    J[2]=dz/rhat;
    
}

void calc_3D_link(
            double *e,
            double *J,
            double *X,
            double *t,   
            int j,
            int i
              )
{
    double ssum,rhat,dx,dy,dz;
    int D=7;

    dx=X[D*i+4]-X[D*j+4];
    dy=X[D*i+5]-X[D*j+5];
    dz=X[D*i+6]-X[D*j+6];
    //printf("Xj %f %f %f %d\n",X[D*j+4],X[D*j+5],X[D*j+6],j);
    //printf("Xi %f %f %f %d\n",X[D*i+4],X[D*i+5],X[D*i+6],i);
    //printf("dx %f %f %f %d %d\n",dx,dy,dz,j,i);
    ssum=dx*dx+dy*dy+dz*dz;
    rhat=sqrt(ssum);
    e[0]=t[0]-rhat;
//    printf("e %f %f %f %d %d\n",e[0],ssum,rhat,j,i);
    /*Jacobian*/
    J[0]=-dx/rhat;
    J[1]=-dy/rhat;
    J[2]=-dz/rhat;
    J[3]=dx/rhat;
    J[4]=dy/rhat;
    J[5]=dz/rhat;
    
}


void motion3Da3(
            double *e,
            double *J,
            double *X,
            double *w, 
            int j,
            int i
              )
{
    double tq[4],*v,dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9],a;
    double R[9],norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;
    int D=7,D2=14;
    
    /*X2=R*(X+t)*/
/* q */

    tq[0]=X[D*j]*w[0]-X[D*j+1]*w[1]-X[D*j+2]*w[2]-X[D*j+3]*w[3];
    tq[1]=X[D*j]*w[1]+X[D*j+1]*w[0]+X[D*j+2]*w[3]-X[D*j+3]*w[2];
    tq[2]=X[D*j]*w[2]+X[D*j+2]*w[0]-X[D*j+1]*w[3]+X[D*j+3]*w[1];
    tq[3]=X[D*j]*w[3]+X[D*j+3]*w[0]+X[D*j+1]*w[2]-X[D*j+2]*w[1];

    

    e[0]=(X[D*i]  -tq[0]);
    e[1]=(X[D*i+1]-tq[1]);
    e[2]=(X[D*i+2]-tq[2]);
    e[3]=(X[D*i+3]-tq[3]);
    
/*t*/
    v=X+D*j;
    norm=1/(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
    q1t2=v[0]*v[0];
    q2t2=v[1]*v[1];
    q3t2=v[2]*v[2];
    q4t2=v[3]*v[3];
    q1q2=v[0]*v[1];
    q2q3=v[1]*v[2];
    q3q4=v[2]*v[3];
    q1q4=v[0]*v[3];
    q2q4=v[1]*v[3];
    q1q3=v[0]*v[2];
    /* building Euclidian transformation matrix */

    R[0]=(q1t2+q2t2-q3t2-q4t2)*norm;
    R[1]=2*(q2q3+q1q4)*norm;
    R[2]=2*(q2q4-q1q3)*norm;
    R[3]=2*(q2q3-q1q4)*norm;
    R[4]=(q1t2-q2t2+q3t2-q4t2)*norm;
    R[5]=2*(q3q4+q1q2)*norm;
    R[6]=2*(q2q4+q1q3)*norm;
    R[7]=2*(q3q4-q1q2)*norm;
    R[8]=(q1t2-q2t2-q3t2+q4t2)*norm;
    a=2*norm;
   
    dRdq0[0]= v[0]*norm*2-a*v[0]*R[0];
    dRdq0[1]= v[3]*norm*2-a*v[0]*R[1];
    dRdq0[2]=-v[2]*norm*2-a*v[0]*R[2];
    dRdq0[3]=-v[3]*norm*2-a*v[0]*R[3];
    dRdq0[4]= v[0]*norm*2-a*v[0]*R[4];
    dRdq0[5]= v[1]*norm*2-a*v[0]*R[5];
    dRdq0[6]= v[2]*norm*2-a*v[0]*R[6];
    dRdq0[7]=-v[1]*norm*2-a*v[0]*R[7];
    dRdq0[8]= v[0]*norm*2-a*v[0]*R[8];

    dRdq1[0]= v[1]*norm*2-a*v[1]*R[0];
    dRdq1[1]= v[2]*norm*2-a*v[1]*R[1];
    dRdq1[2]= v[3]*norm*2-a*v[1]*R[2];
    dRdq1[3]= v[2]*norm*2-a*v[1]*R[3];
    dRdq1[4]=-v[1]*norm*2-a*v[1]*R[4];
    dRdq1[5]= v[0]*norm*2-a*v[1]*R[5];
    dRdq1[6]= v[3]*norm*2-a*v[1]*R[6];
    dRdq1[7]=-v[0]*norm*2-a*v[1]*R[7];
    dRdq1[8]=-v[1]*norm*2-a*v[1]*R[8]; 
     
    dRdq2[0]=-v[2]*norm*2-a*v[2]*R[0];
    dRdq2[1]= v[1]*norm*2-a*v[2]*R[1];
    dRdq2[2]=-v[0]*norm*2-a*v[2]*R[2]; 
    dRdq2[3]= v[1]*norm*2-a*v[2]*R[3];
    dRdq2[4]= v[2]*norm*2-a*v[2]*R[4];
    dRdq2[5]= v[3]*norm*2-a*v[2]*R[5];
    dRdq2[6]= v[0]*norm*2-a*v[2]*R[6];
    dRdq2[7]= v[3]*norm*2-a*v[2]*R[7];
    dRdq2[8]=-v[2]*norm*2-a*v[2]*R[8];
     
    dRdq3[0]=-v[3]*norm*2-a*v[3]*R[0];
    dRdq3[1]= v[0]*norm*2-a*v[3]*R[1];
    dRdq3[2]= v[1]*norm*2-a*v[3]*R[2];
    dRdq3[3]=-v[0]*norm*2-a*v[3]*R[3]; 
    dRdq3[4]=-v[3]*norm*2-a*v[3]*R[4];
    dRdq3[5]= v[2]*norm*2-a*v[3]*R[5];
    dRdq3[6]= v[1]*norm*2-a*v[3]*R[6];
    dRdq3[7]= v[2]*norm*2-a*v[3]*R[7];
    dRdq3[8]= v[3]*norm*2-a*v[3]*R[8];
    e[4]=X[D*i+4]-X[D*j+4]-(R[0]*w[4]+R[3]*w[5]+R[6]*w[6]);
    e[5]=X[D*i+5]-X[D*j+5]-(R[1]*w[4]+R[4]*w[5]+R[7]*w[6]);
    e[6]=X[D*i+6]-X[D*j+6]-(R[2]*w[4]+R[5]*w[5]+R[8]*w[6]);
 

    /*Jacobian*/
    /*j*/
    J[0]=w[0];
    J[1]=-w[1];
    J[2]=-w[2];
    J[3]=-w[3];
    J[4]=0;
    J[5]=0;
    J[6]=0;

    J[7]=-1;
    J[8]=0;
    J[9]=0;
    J[10]=0;
    J[11]=0;
    J[12]=0;
    J[13]=0;

    J[D2]=w[1];
    J[1+D2]=w[0];
    J[2+D2]=w[3];
    J[3+D2]=-w[2];
    J[4+D2]=0;
    J[5+D2]=0;
    J[6+D2]=0;

    J[7+D2]=0;
    J[8+D2]=-1;
    J[9+D2]=0;
    J[10+D2]=0;
    J[11+D2]=0;
    J[12+D2]=0;
    J[13+D2]=0;

    J[2*D2]=w[2];
    J[1+2*D2]=-w[3];
    J[2+2*D2]=w[0];
    J[3+2*D2]=w[1];
    J[4+2*D2]=0;
    J[5+2*D2]=0;
    J[6+2*D2]=0;

    J[7+2*D2]=0;
    J[8+2*D2]=0;
    J[9+2*D2]=-1;
    J[10+2*D2]=0;
    J[11+2*D2]=0;
    J[12+2*D2]=0;
    J[13+2*D2]=0;

    J[3*D2]=w[3];
    J[1+3*D2]=w[2];
    J[2+3*D2]=-w[1];
    J[3+3*D2]=w[0];
    J[4+3*D2]=0;
    J[5+3*D2]=0;
    J[6+3*D2]=0;

    J[7+3*D2]=0;
    J[8+3*D2]=0;
    J[9+3*D2]=0;
    J[10+3*D2]=-1;
    J[11+3*D2]=0;
    J[12+3*D2]=0;
    J[13+3*D2]=0;

    J[4*D2]=(dRdq0[0]*w[4]+dRdq0[3]*w[5]+dRdq0[6]*w[6]);
    J[1+4*D2]=(dRdq1[0]*w[4]+dRdq1[3]*w[5]+dRdq1[6]*w[6]);
    J[2+4*D2]=(dRdq2[0]*w[4]+dRdq2[3]*w[5]+dRdq2[6]*w[6]);
    J[3+4*D2]=(dRdq3[0]*w[4]+dRdq3[3]*w[5]+dRdq3[6]*w[6]);
    J[4+4*D2]=1;
    J[5+4*D2]=0;
    J[6+4*D2]=0;

    J[7+4*D2]=0;
    J[8+4*D2]=0;
    J[9+4*D2]=0;
    J[10+4*D2]=0;
    J[11+4*D2]=-1;
    J[12+4*D2]=0;
    J[13+4*D2]=0;

    J[5*D2]=(dRdq0[1]*w[4]+dRdq0[4]*w[5]+dRdq0[7]*w[6]);
    J[1+5*D2]=(dRdq1[1]*w[4]+dRdq1[4]*w[5]+dRdq1[7]*w[6]);
    J[2+5*D2]=(dRdq2[1]*w[4]+dRdq2[4]*w[5]+dRdq2[7]*w[6]);
    J[3+5*D2]=(dRdq3[1]*w[4]+dRdq3[4]*w[5]+dRdq3[7]*w[6]);
    J[4+5*D2]=0;
    J[5+5*D2]=1;
    J[6+5*D2]=0;

    J[7+5*D2]=0;
    J[8+5*D2]=0;
    J[9+5*D2]=0;
    J[10+5*D2]=0;
    J[11+5*D2]=0;
    J[12+5*D2]=-1;
    J[13+5*D2]=0;

    J[6*D2]=(dRdq0[2]*w[4]+dRdq0[5]*w[5]+dRdq0[8]*w[6]);
    J[1+6*D2]=(dRdq1[2]*w[4]+dRdq1[5]*w[5]+dRdq1[8]*w[6]);
    J[2+6*D2]=(dRdq2[2]*w[4]+dRdq2[5]*w[5]+dRdq2[8]*w[6]);
    J[3+6*D2]=(dRdq3[2]*w[4]+dRdq3[5]*w[5]+dRdq3[8]*w[6]);
    J[4+6*D2]=0;
    J[5+6*D2]=0;
    J[6+6*D2]=1;

    J[7+6*D2]=0;
    J[8+6*D2]=0;
    J[9+6*D2]=0;
    J[10+6*D2]=0;
    J[11+6*D2]=0;
    J[12+6*D2]=0;
    J[13+6*D2]=-1;
    
}

void motion3Db3(
            double *e,
            double *J,
            double *X,
            double *w, 
            int j,
            int i
              )
{
    double tq[4];
    double R[9],norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;

    
    int D=7,D2=14;
    /*X2=R*X+t*/
/* q */

    tq[0]=X[D*j]*w[0]-X[D*j+1]*w[1]-X[D*j+2]*w[2]-X[D*j+3]*w[3];
    tq[1]=X[D*j]*w[1]+X[D*j+1]*w[0]+X[D*j+2]*w[3]-X[D*j+3]*w[2];
    tq[2]=X[D*j]*w[2]+X[D*j+2]*w[0]-X[D*j+1]*w[3]+X[D*j+3]*w[1];
    tq[3]=X[D*j]*w[3]+X[D*j+3]*w[0]+X[D*j+1]*w[2]-X[D*j+2]*w[1];

    

    e[0]=(X[D*i]  -tq[0]);
    e[1]=(X[D*i+1]-tq[1]);
    e[2]=(X[D*i+2]-tq[2]);
    e[3]=(X[D*i+3]-tq[3]);
    
/* t */
    norm=1/(w[0]*w[0]+w[1]*w[1]+w[2]*w[2]+w[3]*w[3]);
    q1t2=w[0]*w[0];
    q2t2=w[1]*w[1];
    q3t2=w[2]*w[2];
    q4t2=w[3]*w[3];
    q1q2=w[0]*w[1];
    q2q3=w[1]*w[2];
    q3q4=w[2]*w[3];
    q1q4=w[0]*w[3];
    q2q4=w[1]*w[3];
    q1q3=w[0]*w[2];
    /* building Euclidian transformation matrix */

    R[0]=(q1t2+q2t2-q3t2-q4t2)*norm;
    R[1]=2*(q2q3+q1q4)*norm;
    R[2]=2*(q2q4-q1q3)*norm;
    R[3]=2*(q2q3-q1q4)*norm;
    R[4]=(q1t2-q2t2+q3t2-q4t2)*norm;
    R[5]=2*(q3q4+q1q2)*norm;
    R[6]=2*(q2q4+q1q3)*norm;
    R[7]=2*(q3q4-q1q2)*norm;
    R[8]=(q1t2-q2t2-q3t2+q4t2)*norm;

    e[4]=(X[D*i+4]-(R[0]*X[D*j+4]+R[3]*X[D*j+5]+R[6]*X[D*j+6]+w[4]));
    e[5]=(X[D*i+5]-(R[1]*X[D*j+4]+R[4]*X[D*j+5]+R[7]*X[D*j+6]+w[5]));
    e[6]=(X[D*i+6]-(R[2]*X[D*j+4]+R[5]*X[D*j+5]+R[8]*X[D*j+6]+w[6]));
 

    /*Jacobian*/
    /*j*/
    J[0]=w[0];
    J[1]=-w[1];
    J[2]=-w[2];
    J[3]=-w[3];
    J[4]=0;
    J[5]=0;
    J[6]=0;

    J[7]=-1;
    J[8]=0;
    J[9]=0;
    J[10]=0;
    J[11]=0;
    J[12]=0;
    J[13]=0;

    J[D2]=w[1];
    J[1+D2]=w[0];
    J[2+D2]=w[3];
    J[3+D2]=-w[2];
    J[4+D2]=0;
    J[5+D2]=0;
    J[6+D2]=0;

    J[7+D2]=0;
    J[8+D2]=-1;
    J[9+D2]=0;
    J[10+D2]=0;
    J[11+D2]=0;
    J[12+D2]=0;
    J[13+D2]=0;

    J[2*D2]=w[2];
    J[1+2*D2]=-w[3];
    J[2+2*D2]=w[0];
    J[3+2*D2]=w[1];
    J[4+2*D2]=0;
    J[5+2*D2]=0;
    J[6+2*D2]=0;

    J[7+2*D2]=0;
    J[8+2*D2]=0;
    J[9+2*D2]=-1;
    J[10+2*D2]=0;
    J[11+2*D2]=0;
    J[12+2*D2]=0;
    J[13+2*D2]=0;

    J[3*D2]=w[3];
    J[1+3*D2]=w[2];
    J[2+3*D2]=-w[1];
    J[3+3*D2]=w[0];
    J[4+3*D2]=0;
    J[5+3*D2]=0;
    J[6+3*D2]=0;

    J[7+3*D2]=0;
    J[8+3*D2]=0;
    J[9+3*D2]=0;
    J[10+3*D2]=-1;
    J[11+3*D2]=0;
    J[12+3*D2]=0;
    J[13+3*D2]=0;

    J[4*D2]=0;
    J[1+4*D2]=0;
    J[2+4*D2]=0;
    J[3+4*D2]=0;
    J[4+4*D2]=R[0];
    J[5+4*D2]=R[3];
    J[6+4*D2]=R[6];

    J[7+4*D2]=0;
    J[8+4*D2]=0;
    J[9+4*D2]=0;
    J[10+4*D2]=0;
    J[11+4*D2]=-1;
    J[12+4*D2]=0;
    J[13+4*D2]=0;

    J[5*D2]=0;
    J[1+5*D2]=0;
    J[2+5*D2]=0;
    J[3+5*D2]=0;
    J[4+5*D2]=R[1];
    J[5+5*D2]=R[4];
    J[6+5*D2]=R[7];

    J[7+5*D2]=0;
    J[8+5*D2]=0;
    J[9+5*D2]=0;
    J[10+5*D2]=0;
    J[11+5*D2]=0;
    J[12+5*D2]=-1;
    J[13+5*D2]=0;

    J[6*D2]=0;
    J[1+6*D2]=0;
    J[2+6*D2]=0;
    J[3+6*D2]=0;
    J[4+6*D2]=R[2];
    J[5+6*D2]=R[5];
    J[6+6*D2]=R[8];

    J[7+6*D2]=0;
    J[8+6*D2]=0;
    J[9+6*D2]=0;
    J[10+6*D2]=0;
    J[11+6*D2]=0;
    J[12+6*D2]=0;
    J[13+6*D2]=-1;
    
}


void motion3Db30(
            double *e,
            double *J,
            double *X0,
            double *X,
            double *w,
            int i
              )
{
    double tq[4];
    double R[9],norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4;

    
    int D=7,D2=14;
    /*X2=R*X+t*/
/* q */

    tq[0]=X0[0]*w[0]-X0[1]*w[1]-X0[2]*w[2]-X0[3]*w[3];
    tq[1]=X0[0]*w[1]+X0[1]*w[0]+X0[2]*w[3]-X0[3]*w[2];
    tq[2]=X0[0]*w[2]+X0[2]*w[0]-X0[1]*w[3]+X0[3]*w[1];
    tq[3]=X0[0]*w[3]+X0[3]*w[0]+X0[1]*w[2]-X0[2]*w[1];

    

    e[0]=(X[D*i]  -tq[0]);
    e[1]=(X[D*i+1]-tq[1]);
    e[2]=(X[D*i+2]-tq[2]);
    e[3]=(X[D*i+3]-tq[3]);
    
/* t */
    norm=1/(w[0]*w[0]+w[1]*w[1]+w[2]*w[2]+w[3]*w[3]);
    q1t2=w[0]*w[0];
    q2t2=w[1]*w[1];
    q3t2=w[2]*w[2];
    q4t2=w[3]*w[3];
    q1q2=w[0]*w[1];
    q2q3=w[1]*w[2];
    q3q4=w[2]*w[3];
    q1q4=w[0]*w[3];
    q2q4=w[1]*w[3];
    q1q3=w[0]*w[2];
    /* building Euclidian transformation matrix */

    R[0]=(q1t2+q2t2-q3t2-q4t2)*norm;
    R[1]=2*(q2q3+q1q4)*norm;
    R[2]=2*(q2q4-q1q3)*norm;
    R[3]=2*(q2q3-q1q4)*norm;
    R[4]=(q1t2-q2t2+q3t2-q4t2)*norm;
    R[5]=2*(q3q4+q1q2)*norm;
    R[6]=2*(q2q4+q1q3)*norm;
    R[7]=2*(q3q4-q1q2)*norm;
    R[8]=(q1t2-q2t2-q3t2+q4t2)*norm;


    e[4]=(X[D*i+4]-(R[0]*X0[4]+R[3]*X0[5]+R[6]*X0[6]+w[4]));
    e[5]=(X[D*i+5]-(R[1]*X0[4]+R[4]*X0[5]+R[7]*X0[6]+w[5]));
    e[6]=(X[D*i+6]-(R[2]*X0[4]+R[5]*X0[5]+R[8]*X0[6]+w[6]));


    /*Jacobian*/
    /*j*/

    J[0]=-1;
    J[1]=0;
    J[2]=0;
    J[3]=0;
    J[4]=0;
    J[5]=0;
    J[6]=0;


    J[0+D2]=0;
    J[1+D2]=-1;
    J[2+D2]=0;
    J[3+D2]=0;
    J[4+D2]=0;
    J[5+D2]=0;
    J[6+D2]=0;


    J[0+2*D2]=0;
    J[1+2*D2]=0;
    J[2+2*D2]=-1;
    J[3+2*D2]=0;
    J[4+2*D2]=0;
    J[5+2*D2]=0;
    J[6+2*D2]=0;


    J[0+3*D2]=0;
    J[1+3*D2]=0;
    J[2+3*D2]=0;
    J[3+3*D2]=-1;
    J[4+3*D2]=0;
    J[5+3*D2]=0;
    J[6+3*D2]=0;


    J[0+4*D2]=0;
    J[1+4*D2]=0;
    J[2+4*D2]=0;
    J[3+4*D2]=0;
    J[4+4*D2]=-1;
    J[5+4*D2]=0;
    J[6+4*D2]=0;


    J[0+5*D2]=0;
    J[1+5*D2]=0;
    J[2+5*D2]=0;
    J[3+5*D2]=0;
    J[4+5*D2]=0;
    J[5+5*D2]=-1;
    J[6+5*D2]=0;

 
    J[0+6*D2]=0;
    J[1+6*D2]=0;
    J[2+6*D2]=0;
    J[3+6*D2]=0;
    J[4+6*D2]=0;
    J[5+6*D2]=0;
    J[6+6*D2]=-1;


    
}



void slam3D_iter_mod0(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *u,
        double *W,
        double *S,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[7],q[4],t[3],norm;
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;


    for(j=0;j<N;j++) /* for each measurement */
    {

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
            norm=sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
            if(q[0]<0)
                norm=-norm;
            q[0]=q[0]/norm;
            q[1]=q[1]/norm;
            q[2]=q[2]/norm;
            q[3]=q[3]/norm;
            Xpri[frame*7]=q[0];
            Xpri[frame*7+1]=q[1];
            Xpri[frame*7+2]=q[2];
            Xpri[frame*7+3]=q[3];

        /*calculate Jacobian and residual */

            proj3D_fun(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);

        /*calculate covariance matrix multiplications */

            calc_Ce(Ce, C, e, S[0], indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C, S[0], Nv, Nx, 3, indi, 7*frame);
            calc_Ce(Ce, C+10, e+1, S[0], indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C+10, S[0], Nv,Nx, 3, indi, 7*frame);

        /* measurement error */

            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        

            proj3D_funs(e,C,q,t,p,z+2*j,Xpri+Nv,indi,th);

    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, S[0], indi, Nv);
            calc_CCs(xx, C, S[0], 3, indi);
            calc_Ces(Ce, C+3, e+1, S[0], indi, Nv);
            calc_CCs(xx, C+3, S[0], 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
          
        }
    }

    motion3Db30(e,C,X0+start_frame*7,Xpri,u,0);
    calc3Dlink_Ce(Ce,C,W,e,0,7,7);
    calc3Dlink_CC(vv,Nv,C,W,0,7,7);
       // er[N]=(e[0]*e[0]+e[1]*e[1]+e[2]*e[2]+e[3]*e[3]+e[4]*e[4]+e[5]*e[5]+e[6]*e[6]+e[7]*e[7]+e[8]*e[8])+e[9]*e[9];
    er[N]=e[0];
    er[N+1]=e[1];
    er[N+2]=e[2];
    er[N+3]=e[3];
    er[N+4]=e[4];
    er[N+5]=e[5];
    er[N+6]=e[6];
    
    for(j=0;j<num_frames-2;j++) 
    {
        motion3Db3(e,C,Xpri,u+(j+1)*7,j,j+1);
        calc3Dlink_Ce(Ce,C,W,e,j*7,7,14);
        calc3Dlink_CC(vv,Nv,C,W,j*7,7,14);
       // er[N+j+1]=(e[0]*e[0]+e[1]*e[1]+e[2]*e[2]+e[3]*e[3]+e[4]*e[4]+e[5]*e[5]+e[6]*e[6]+e[7]*e[7]+e[8]*e[8])+e[9]*e[9];
       er[N+j*7+7]=e[0];
       er[N+j*7+8]=e[1];
       er[N+j*7+9]=e[2];
       er[N+j*7+10]=e[3];
       er[N+j*7+11]=e[4];
       er[N+j*7+12]=e[5];
       er[N+j*7+13]=e[6];

       
    }
 /* jo */
        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);
            
            slam_vx_icb(icb,vx,R,Nv,Nx,Nv);
            /*for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];*/

            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}
void slam3D_iter_mod_s(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *u,
        double *W,
        double *S,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[7],q[4],t[3],norm;
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;


    for(j=0;j<N;j++) /* for each measurement */
    {

        if(indf[j]>0)
        {
            /* indi is the index to the landmark that corresponds to measurement i*/
            indi=ind[j];
            frame=indf[j];
            frame--;
            indi--;
            q[0]=Xpri[frame*7];
            q[1]=Xpri[frame*7+1];
            q[2]=Xpri[frame*7+2];
            q[3]=Xpri[frame*7+3];
            t[0]=Xpri[frame*7+4];
            t[1]=Xpri[frame*7+5];
            t[2]=Xpri[frame*7+6];
            norm=sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
            if(q[0]<0)
                norm=-norm;
            q[0]=q[0]/norm;
            q[1]=q[1]/norm;
            q[2]=q[2]/norm;
            q[3]=q[3]/norm;
            Xpri[frame*7]=q[0];
            Xpri[frame*7+1]=q[1];
            Xpri[frame*7+2]=q[2];
            Xpri[frame*7+3]=q[3];

        /*calculate Jacobian and residual */

            proj3D_fun1(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);

        /*calculate covariance matrix multiplications */

            calc_Ce(Ce, C, e, S[0], indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C, S[0], Nv, Nx, 3, indi, 7*frame);
            calc_Ce(Ce, C+10, e+1, S[0], indi, 7*frame, Nv);
            calc_CC(vv,vx,xx, C+10, S[0], Nv,Nx, 3, indi, 7*frame);

        /* measurement error */

            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
        }
        else
        {
            indi=ind[j];
            frame=-indf[j];
            indi--;
                  
            q[0]=X0[frame*7];
            q[1]=X0[frame*7+1];
            q[2]=X0[frame*7+2];
            q[3]=X0[frame*7+3];
            t[0]=X0[frame*7+4];
            t[1]=X0[frame*7+5];
            t[2]=X0[frame*7+6];
        

            proj3D_funs1(e,C,q,t,p,z+2*j,Xpri+Nv,indi,th);

    /*calculate sparse covariance matrix multiplications */
            calc_Ces(Ce, C, e, S[0], indi, Nv);
            calc_CCs(xx, C, S[0], 3, indi);
            calc_Ces(Ce, C+3, e+1, S[0], indi, Nv);
            calc_CCs(xx, C+3, S[0], 3, indi);
            er[j]=e[0]*e[0]+e[1]*e[1];
            (*ssd)+=er[j];
 
        }
    }
    calc_3D_link0(e,C,X0+(start_frame-1)*7,Xpri,u,0);
    calc3Dlink_Ce(Ce,C,W,e,4,1,3);
    calc3Dlink_CC(vv,Nv,C,W,4,1,3);
    
    er[N]=e[0]*e[0]*W[0]*W[0];
    (*ssd)+=er[N];
    
    for(j=0;j<num_frames-2;j++) 
    {
        calc_3D_link(e,C,Xpri,u+(j+1),j,j+1);
        calc3Dlink_Ce(Ce,C,W,e,j*7+4,1,3);
        calc3Dlink_CC(vv,Nv,C,W,j*7+4,1,3);
        calc3Dlink_Ce(Ce,C+3,W,e,j*7+11,1,3);
        calc3Dlink_CC(vv,Nv,C+3,W,j*7+11,1,3);
       // er[N+j+1]=(e[0]*e[0]+e[1]*e[1]+e[2]*e[2]+e[3]*e[3]+e[4]*e[4]+e[5]*e[5]+e[6]*e[6]+e[7]*e[7]+e[8]*e[8])+e[9]*e[9];
       er[N+j+1]=e[0]*e[0]*W[0]*W[0];
       (*ssd)+=er[N+j+1];

       
    }
 /* jo */
        /* should check if previous move was a success before next */
    if(Nv>0&&N>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);
            
            slam_vx_icb(icb,vx,R,Nv,Nx,Nv);
            /*for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];*/

            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
    {
        printf("error: %d %d \n",N,Nv);
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];
    }

}


void slam3D_iter_map(
        double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double *X0,
        double *Xpos,
        double *Xpri,
        double *map,
        int *ind,
        int *indf,
        double *z,
        int *indm,
        int *indfm,
        double *zm,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int Nm,
        int num_frames,
        double lambda,
        double *ssd,
        double *er,
        double th
        )
{
    double e[2],q[4],t[3];
    int i,j,k,indi,ret,frame,Nv,Nx;
 
    Nv=(num_frames-1)*7;
    Nx=D-Nv;
    k=0;
    (*ssd)=0;
    for(j=0;j<Nv*Nv;j++)
        vv[j]=0;
    for(j=0;j<Nv*Nx;j++)
        vx[j]=0;
    for(j=0;j<Nx*3;j++)
        xx[j]=0;
    for(j=0;j<D;j++)
        Ce[j]=0;

    for(j=0;j<N;j++) /* for each measurement */
    {
        frame=indf[j];
        
        q[0]=Xpri[frame*7];
        q[1]=Xpri[frame*7+1];
        q[2]=Xpri[frame*7+2];
        q[3]=Xpri[frame*7+3];
        t[0]=Xpri[frame*7+4];
        t[1]=Xpri[frame*7+5];
        t[2]=Xpri[frame*7+6];
        /* indi is the index to the landmark that corresponds to measurement i*/
        indi=ind[j];
        indi--;
        /*calculate Jacobian and residual */

        proj3D_fun(e,C,q,t,p,z+j*2,Xpri+Nv,indi,th);
        /*calculate covariance matrix multiplications */

        calc_Ce(Ce, C, e, 1., indi, 7*frame, Nv);
        calc_CC(vv,vx,xx, C, 1., Nv, Nx, 3, indi, 7*frame);
        calc_Ce(Ce, C+10, e+1, 1., indi, 7*frame, Nv);
        calc_CC(vv,vx,xx, C+10, 1., Nv,Nx, 3, indi, 7*frame);

        /* measurement error */
        er[j]=e[0]*e[0]+e[1]*e[1];
        (*ssd)+=er[j];
    }
    for(j=0;j<Nm;j++) /* for each measurement */
    {
        frame=indfm[j];
        
        q[0]=Xpri[frame*7];
        q[1]=Xpri[frame*7+1];
        q[2]=Xpri[frame*7+2];
        q[3]=Xpri[frame*7+3];
        t[0]=Xpri[frame*7+4];
        t[1]=Xpri[frame*7+5];
        t[2]=Xpri[frame*7+6];
        /* indi is the index to the landmark that corresponds to measurement i*/
        indi=indm[j];
        indi--;
        /*calculate Jacobian and residual */

        proj3D_funp(e,C,q,t,p,zm+j*2,map,indi,th);

        /*calculate covariance matrix multiplications */

        calc_Ced(Ce, C, e, 1., frame*7, 7);
        calc_CCd(vv, C, 1., Nv, 7,  7*frame);
        calc_Ced(Ce, C+7, e+1, 1., frame*7, 7);
        calc_CCd(vv, C+7, 1., Nv, 7, 7*frame);

        /* measurement error */
        er[j]=e[0]*e[0]+e[1]*e[1];
        (*ssd)+=er[j];
    }

        /* should check if previous move was a success before next */
    if(Nv>0)
    {
        do
        {
            for(j=0;j<Nv*Nv;j++)
                R[j]=vv[j];
            for(j=0;j<Nx*3;j++)
                L[j]=xx[j];
            for(j=0;j<Nx;j++)
                L[j*3]+=lambda;
            for(j=0;j<Nv;j++)
                R[j+j*Nv]+=lambda;
            /* go */
            ret=choldcb(L, L, Nx,3);        
            for(i=0;i<Nv;i++)
                cholslb(L,vx+i*Nx,icb+i*Nx,Nx,3);

            for(j=0;j<Nv;j++)
                for(i=0;i<Nv;i++)
                    for(k=0;k<Nx;k++)
                        R[i*Nv+j]=R[i*Nv+j]-vx[i*Nx+k]*icb[j*Nx+k];

            cholslb(L,Ce+Nv,icb,Nx,3);
            for(i=0;i<Nv;i++)
            {
                x[i]=Ce[i];
                for(k=0;k<Nx;k++)
                    x[i]-=vx[i*Nx+k]*icb[k];
            }
            choldc(R, R, Nv);
            cholsl(R,x,g,Nv);
    
            for(i=0;i<Nx;i++)
            {
                x[i]=Ce[i+Nv];
                for(k=0;k<Nv;k++)
                    x[i]-=vx[k*Nx+i]*g[k];
            }
            cholslb(L,x,g+Nv,Nx,3);
        }
        while(ret<0);
        
        for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];
    }
    else
        for(j=0;j<D;j++)Xpos[j]=Xpri[j];

}

void slam3D_batch(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;

    slam3D_iter_batch(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_batch(L,R,x,icb,g,p,X0,Xpos1,Xpos,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin: %d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_batch(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}
void slam3D_stereo_batch(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *pl,
        double *pr,
        double *stereo_par,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        int *indc,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;

    slam3D_iter_stereo_batch(L,R,x,icb,g,pl,pr,stereo_par,X0,Xpos,X,ind,indf,indc,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_stereo_batch(L,R,x,icb,g,pl,pr,stereo_par,X0,Xpos1,Xpos,ind,indf,indc,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin: %d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_stereo_batch(L,R,x,icb,g,pl,pr,stereo_par,X0,Xpos,X,ind,indf,indc,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}
void slam3D_struc_stereo_batch(double *L,
        double *g,
        double *pl,
        double *pr,
        double *stereo_par,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indc,
        double *z,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;

    slam3D_iter_struc_stereo_batch(L,g,pl,pr,stereo_par,X0,Xpos,X,ind,indc,z,xx,Ce,C,D,N,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_struc_stereo_batch(L,g,pl,pr,stereo_par,X0,Xpos1,Xpos,ind,indc,z,xx,Ce,C,D,N,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin: %d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_struc_stereo_batch(L,g,pl,pr,stereo_par,X0,Xpos,X,ind,indc,z,xx,Ce,C,D,N,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}

void slam3D_batch1(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;

    slam3D_iter_batch1(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_batch1(L,R,x,icb,g,p,X0,Xpos1,Xpos,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin: %d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_batch1(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}

void slam3D_batcht(double *L,
		double *R,
		double *x,
		double *icb,
		double *g,
		double *p,
		double **Xout,
		double *X0,
		double *Xpri,
		int *ind,
		int *indf,
		double *z,
		double *vv,
		double *vx,
		double *xx,
		double *Ce,
		double *C,
		int D,
		int N,
		int num_frames,
		double lambda,
		double *er,
		double th,
		int cyc
		)
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;
/*    printf("Xpos=[");
	for(j=0;j<D;j++)
            printf("%lf ",X[j]);
	printf("]\n");  */
    slam3D_ini(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,lambda,&summin,er,th);

    printf("summin:%f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_ini(L,R,x,icb,g,p,X0,Xpos1,Xpos,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin:%f\n",summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_ini(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,vv,vx,xx,Ce,C,D,N,num_frames,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}


void slam3D_batcht_map(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double **Xout,
        double *X0,
        double *Xpri,
        double *map,
        int *ind,
        int *indf,
        double *z,
        int *indm,
        int *indfm,
        double *zm,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int Nm,
        int num_frames,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;

    slam3D_iter_map(L,R,x,icb,g,p,X0,Xpos,X,map,ind,indf,z,indm,indfm,zm,vv,vx,xx,Ce,C,D,N,Nm,num_frames,lambda,&summin,er,th);

    printf("summin:%f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_map(L,R,x,icb,g,p,X0,Xpos1,Xpos,map,ind,indf,z,indm,indfm,zm,vv,vx,xx,Ce,C,D,N,Nm,num_frames,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin:%f\n",summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_map(L,R,x,icb,g,p,X0,Xpos,X,map,ind,indf,z,indm,indfm,zm,vv,vx,xx,Ce,C,D,N,Nm,num_frames,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}

void slam3D_batch_mod0(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *u,
        double *W,
        double *P,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;
/*    printf("Xpos=[");
    for(j=0;j<D;j++)
            printf("%lf ",X[j]);
    printf("]\n");  */
    slam3D_iter_mod0(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_mod0(L,R,x,icb,g,p,X0,Xpos1,Xpos,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin:%d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_mod0(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}

void slam3D_batch_mod_s(double *L,
        double *R,
        double *x,
        double *icb,
        double *g,
        double *p,
        double **Xout,
        double *X0,
        double *Xpri,
        int *ind,
        int *indf,
        double *z,
        double *u,
        double *W,
        double *P,
        double *vv,
        double *vx,
        double *xx,
        double *Ce,
        double *C,
        int D,
        int N,
        int num_frames,
        int start_frame,
        double lambda,
        double *er,
        double th,
        int cyc
        )
{
    double summin,sume,*Xpos,*X,*Xpos1,*pX;
    int j;

    X=Xpri;
    Xpos=X+D;
    Xpos1=X+2*D;
/*    printf("Xpos=[");
    for(j=0;j<D;j++)
            printf("%lf ",X[j]);
    printf("]\n");  */
    slam3D_iter_mod_s(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&summin,er,th);

    printf("summin: -1 %f\n",summin);
    for(j=0;j<cyc;j++)
    {
        slam3D_iter_mod_s(L,R,x,icb,g,p,X0,Xpos1,Xpos,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        printf("sume:%f\n",sume);
        if(sume<summin)
        {
            lambda=lambda/10.;
            summin=sume;
            pX=X;
            X=Xpos;
            Xpos=Xpos1;
            Xpos1=pX;
            printf("summin:%d %f\n",j,summin);
        }
        else
        {
            lambda=lambda*12.;
            slam3D_iter_mod_s(L,R,x,icb,g,p,X0,Xpos,X,ind,indf,z,u,W,P,vv,vx,xx,Ce,C,D,N,num_frames,start_frame,lambda,&sume,er,th);
        }
    }
    (*Xout)=X;
}
